/*
根据手眼标定指令(来自hand_eye.py/hand_eye_cli())保存标定数据
*/

#include<ros/ros.h>
#include <visualization_msgs/Marker.h>
#include "ti5arm/ti5_targetPos.h"

#include<iostream>
#include<fstream>
#include<ctime>

class ARUCO_H_E
{
private:
    /* data */
    std::ofstream m_outf;
    std::string m_saveFileName;
    ros::NodeHandle nh;
    ros::Subscriber hand_eye_cmd_sub;

    

    void hand_eye_cmd_callback(const ti5arm::ti5_targetPos::ConstPtr &);
    void save_data_to_file(const ti5arm::ti5_targetPos::ConstPtr &hand_eye_cmd_msg,geometry_msgs::Pose pose);
public:
    ARUCO_H_E(std::string);
    ~ARUCO_H_E();
};

ARUCO_H_E::ARUCO_H_E(std::string filename)
{
    hand_eye_cmd_sub = nh.subscribe<ti5arm::ti5_targetPos>("hand_eye_cmd",1,&ARUCO_H_E::hand_eye_cmd_callback,this);
    this->m_saveFileName = filename;

    m_outf.open(m_saveFileName,std::ios::out);
    if (!m_outf.is_open())
    {
        std::cout<<"open saveFile failed, please retry\n";
    }
    m_outf << "{\"name\": \"armJ_handeyeXYZwxyz\", \"seq\": \"jointDeg_arucoXYZwxyz\"}\n";
    m_outf.flush();
}

ARUCO_H_E::~ARUCO_H_E()
{
    m_outf.close();
}




void ARUCO_H_E::hand_eye_cmd_callback(const ti5arm::ti5_targetPos::ConstPtr &hand_eye_cmd_msg)
{
    std::cout<<"hand_eye_cmd_msg"<<hand_eye_cmd_msg->cmdList[0]<<std::endl;
    if (hand_eye_cmd_msg->cmdList[0]=='2')
    {
        // 等待aruco码的位姿消息
        boost::shared_ptr<visualization_msgs::Marker const> marker_msg = ros::topic::waitForMessage<visualization_msgs::Marker>("/aruco_single/marker",ros::Duration(5));
        
        if (marker_msg !=NULL)
        {
            /* code */
            int id = marker_msg->id;
            geometry_msgs::Pose pose = marker_msg->pose;
            save_data_to_file(hand_eye_cmd_msg, pose);
            std::cout<<"maker_msg id:"<<id<<std::endl;
            std::cout<<"maker_msg pose:"<<pose<<std::endl;
            std::cout<<"arm pose:"  << hand_eye_cmd_msg->joint1RadList.at(0) << ", " \
                                    << hand_eye_cmd_msg->joint2RadList.at(0) << ", " \
                                    << hand_eye_cmd_msg->joint3RadList.at(0) << ", " \
                                    << hand_eye_cmd_msg->joint4RadList.at(0) << ", " \
                                    << hand_eye_cmd_msg->joint5RadList.at(0) << ", " \
                                    << hand_eye_cmd_msg->joint6RadList.at(0) << std::endl<<std::endl; 
        }
        else
        {
            std::cout << "get aruco marker pose failed! please retry\n";
        }
    }
    else if (hand_eye_cmd_msg->cmdList[0]=='1')
    {
        std::cout << "data  saveed in " << this->m_saveFileName << std::endl;
    }
    
    
}

void ARUCO_H_E::save_data_to_file(const ti5arm::ti5_targetPos::ConstPtr &hand_eye_cmd_msg,geometry_msgs::Pose pose)
{
   

    if (!m_outf.is_open())
    {
        std::cout<<"saveFile not open,please retry\n";
        return;
    }
    
    m_outf << hand_eye_cmd_msg->joint1RadList.at(0) << ", " \
           << hand_eye_cmd_msg->joint2RadList.at(0) << ", " \
           << hand_eye_cmd_msg->joint3RadList.at(0) << ", " \
           << hand_eye_cmd_msg->joint4RadList.at(0) << ", " \
           << hand_eye_cmd_msg->joint5RadList.at(0) << ", " \
           << hand_eye_cmd_msg->joint6RadList.at(0) << ", " \
           << pose.position.x << ", " \
           << pose.position.y << ", " \
           << pose.position.z << ", " \
           << pose.orientation.w << ", " \
           << pose.orientation.x << ", " \
           << pose.orientation.y << ", " \
           << pose.orientation.z << "\n" ;
    m_outf.flush();

}

int main(int argc,  char** argv)
{
    /* code */
    ros::init(argc, argv, "aruco_hand_eye_node");


    time_t now_time;
    time(& now_time);
    tm* time_p = localtime(&now_time);

    std::stringstream file_dir;
    file_dir<<"/home/wheeltec/Desktop/ros_cam/aruco_hand_eye_cli_file" 
            << time_p->tm_year-100 
            << time_p->tm_mon+1
            << time_p->tm_mday
            << time_p->tm_hour
            << time_p->tm_min
            << ".txt";
    std::cout << "data will saveed in " << file_dir.str() << std::endl;

    ARUCO_H_E ahe(file_dir.str());

    ros::Rate loop_rate(100);//100Hz
    std::cout<<"start spin\n";
    while (ros::ok())
    {
        ros::spinOnce();
        loop_rate.sleep();
    }
    return 0;
}




